/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package teamBlitz;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.CANJaguar.SpeedReference;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
import teamBlitz.Court;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */




/* 6DOF DIGITAL:	
 * ATmega328 w/ 16MHz external crystal	
 * ADXL345 accelerometer	
 * ITG3200 gyro	Ouptuts triple axis acceleration and gyro values.	
 * Units are in degrees per second for gyro and g's (1g=9.8m/s^2) for accelerometer.	
 * HARDWARE CONFIGURATION:	
 * Write commands are 0xD0 for gyro and 0xA6 for accel	
 * Read command are 0xD1 for gyro and 0xA7 for accel
 * 
 */


public class Robot2012 extends SimpleRobot 
{
   // SerialPort serial;
//  public static Court myCourt;
    private long time1;

    protected void robotInit() { 
        System.out.println("TeamBlitz robotInit() method running");
//     myCourt=new Court(this);
//       myCourt.init();
    }
    
    Joystick xbox = new Joystick(1);
//    Gyro gyro = new Gyro(1);
    //ADXL345_I2C accel = new ADXL345_I2C(1, ADXL345_I2C.DataFormat_Range.k2G);
//    ADXL345_I2C accel;
    RobotDrive mydrive;
    CANJaguar frontRight = new CANJaguar(2, CANJaguar.ControlMode.kPercentVbus);
    CANJaguar backRight  = new CANJaguar(3, CANJaguar.ControlMode.kPercentVbus);
    CANJaguar backLeft   = new CANJaguar(4, CANJaguar.ControlMode.kPercentVbus);
    CANJaguar frontLeft  = new CANJaguar(5, CANJaguar.ControlMode.kPercentVbus);
    
    
    CANJaguar shooter1   = new CANJaguar(10, CANJaguar.ControlMode.kSpeed);
    CANJaguar shooter2   = new CANJaguar(13, CANJaguar.ControlMode.kSpeed);
    CANJaguar shooter3   = new CANJaguar(12, CANJaguar.ControlMode.kSpeed);
    CANJaguar thruster   = new CANJaguar(11, CANJaguar.ControlMode.kVoltage);
    
   // CANJaguar ramp       = new CANJaguar(15, CANJaguar.ControlMode.kVoltage);
    
    
    Relay picker = new Relay(1);
    
    
    double time = 0;
    double vx = 0;
    double vy = 0;
    double vz = 0;
    double x = 0;
    double y = 0;
    double z = 0;

    public Robot2012() throws CANTimeoutException {
        System.out.println("We got here");

//        try {
//            accel = new ADXL345_I2C(2, ADXL345_I2C.DataFormat_Range.k2G);
//        } catch (Exception e) {
//            System.out.println(e + " " + e.getMessage());
//        }

//        gyro.reset();

        frontRight.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        frontRight.configEncoderCodesPerRev(360);
        frontRight.setVoltageRampRate(25);
        frontRight.configFaultTime(0.5);
        frontRight.enableControl();
        backRight.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        backRight.configEncoderCodesPerRev(250);
        backRight.setVoltageRampRate(25);
        backRight.configFaultTime(0.5);
        backRight.enableControl();
        backLeft.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        backLeft.configEncoderCodesPerRev(360);
        backLeft.setVoltageRampRate(25);
        backLeft.configFaultTime(0.5);
        backLeft.enableControl();
        frontLeft.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        frontLeft.configEncoderCodesPerRev(360);
        frontLeft.setVoltageRampRate(25);
        frontLeft.configFaultTime(0.5);
        frontLeft.enableControl();
       
        
             
        picker.setDirection(Relay.Direction.kBoth);

        shooter1.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        shooter1.configEncoderCodesPerRev(360);
        shooter1.enableControl();
        shooter2.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        shooter2.configEncoderCodesPerRev(360);
        shooter2.enableControl();
        shooter3.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
        shooter3.configEncoderCodesPerRev(360);
        shooter3.enableControl();
        thruster.enableControl();
        
//        ramp.enableControl();
                      SmartDashboard.putInt("Shooting", 0);
//              frontRight..setPID(0, 0, 0);
//            backRight.setPID(0, 0, 0);
//            backLeft.setPID(0, 0, 0);
//            frontLeft.setPID(0, 0, 0);
                     
        mydrive= new RobotDrive(frontLeft, backLeft, frontRight, backRight);
      //  mydrive.setSafetyEnabled(true);
        mydrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
       mydrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        
        System.out.println("initialized everything");
    }

    protected void disabled() {
        System.out.println("teamblitz disabled() method running");
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        try {


            shooter1.setPID(2, 0.01, 0);
            shooter2.setPID(2, 0.01, 0);
            shooter3.setPID(2, 0.01, 0);
//            
            thruster.setPID(3, 0.1, 0);
            
        } catch (Exception e) {
            System.out.println(e.getMessage());
        }
        Shoot(1900);
        picker.set(Relay.Value.kForward);
        Timer.delay(4);
        picker.set(Relay.Value.kOff);
        Timer.delay(2);
        Shoot(2900);
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        System.out.println("got to operatorControl");
//        Court.state myloc=Court.getLocation();
        try {


            shooter1.setPID(2, 0.01, 0);
            shooter2.setPID(2, 0.01, 0);
            shooter3.setPID(2, 0.01, 0);
//            
            thruster.setPID(3, 0, 0);
            

        } catch (Exception e) {
            System.out.println(e.getMessage());
        }

//        gyro.reset();

        int loopcount = 100;
        long last = 0;
        double loopinterval = 0.01;
        double rotConstant = 180;
        int iocount = 0;
        int[][] speeds = new int[4][loopcount];
        double plotScale = 3.0;
        double shooterSpeed = 2500.0;
        
        boolean goingOut = false;
        boolean out = false;
        boolean goingIn = false;
        
        

        int state = 0; // 0 - Control   1 - Stop for shoot    2 - align for shoot    3 - shoot


       while ( isOperatorControl() && isEnabled()) {

//            double ang = gyro.getAngle() * Math.PI / 180;
//
//
//
            
 //
            boolean aButton = xbox.getRawButton(1);
            boolean bButton = xbox.getRawButton(2);
            boolean xButton = xbox.getRawButton(3);
            boolean yButton = xbox.getRawButton(4);
            boolean backButton = xbox.getRawButton(7);
            boolean startButton = xbox.getRawButton(8);
//
//
//
            boolean leftButton = xbox.getRawButton(5);
            boolean rightButton = xbox.getRawButton(6);

//
//            double nX = Math.cos(-ang) * X - Math.sin(-ang) * Y;
//            double nY = Math.sin(-ang) * X + Math.cos(-ang) * Y;
//
//            X = nX;
//            Y = nY;

//
//
//



//        try {
//            int fire = NetworkTable.getTable("SmartDashboard").getInt("Shooting");
//            if (fire!=0) {
//                System.out.println("fire - bang="+fire);
//                Shoot(fire);
//                //myTargets.addShot(distanceFromBasket, a.nextInt() % 4000, 0, a.nextInt() % 5);
//                SmartDashboard.putInt("Shooting", 0);
//            } else {
//                //System.out.println("NOT fire - bang");
//            }
//
//        } catch (NetworkTableKeyNotDefined ex) {
//            ex.printStackTrace();
//        }
//System.out.println(("2x="+X+ "  Y="+Y+"   rot="+rot));

     

            try {
                   if (aButton) {
//                       frontLeft.setX(.75);
                      thruster.setX(12);
//                       System.out.println("faults="+ frontLeft.getFaults());
//                       frontLeft.getReverseLimitOK();
//                       System.out.println("rev="+ frontLeft.getReverseLimitOK());
//                       System.out.println("fwd="+ frontLeft.getForwardLimitOK());
//                       System.out.println("spd="+ frontLeft.getSpeed());
//                       System.out.println("cm="+ frontLeft.getControlMode());
//                       System.out.println("p="+ frontLeft.getP());
//                       System.out.println("pos="+ frontLeft.getPosition());
//                       SpeedReference sr=frontLeft.getSpeedReference();
//                       
                       
                       
                       //System.out.println("spd="+ frontLeft.);
                       
                     }
//                if (startButton) {
//                    ramp.setX(-12);
//                } else if (backButton) {
//                    ramp.setX(12);
//                } else {
//                    ramp.setX(0);
//                }
                if (rightButton || leftButton || bButton) {
                    if (leftButton) shooterSpeed = 1800;
                    else if (rightButton) shooterSpeed = 3000;
                    else if (bButton) shooterSpeed = 1900;
                    //Shoot(shooterSpeed);
                    shooter1.setX(-shooterSpeed);
                    shooter2.setX(-(shooterSpeed));
                    shooter3.setX(-shooterSpeed);
                         thruster.setX(-12.0);
                    //                    if (Math.abs((shooter1.getSpeed()-shooterSpeed)/shooterSpeed) < 0.02
//                      && Math.abs((shooter2.getSpeed()-(shooterSpeed))/(shooterSpeed)) < 0.02
//                      && Math.abs((shooter3.getSpeed()-shooterSpeed)/shooterSpeed) < 0.02) {
//                         thruster.setX(-12.0);
//                         Timer.delay(1.0);
//                    }
                } else {
                    shooter1.setX(0);
                    shooter2.setX(0);
                    shooter3.setX(0);
                    thruster.setX(0);
                }
            } catch (Exception e) {
            }

            try {
                
                
                if (yButton) {
                    picker.set(Relay.Value.kForward);
                    time1=System.currentTimeMillis();
                } else if (xButton) {
                    picker.set(Relay.Value.kReverse);
                    time1=System.currentTimeMillis();
                } else {
                    
                    if(time1+4000<System.currentTimeMillis()){
                    time1=0;    
                    picker.set(Relay.Value.kOff);
                    }
                }

                if (bButton) {
                    //state = 1;
                }

            double X = xbox.getX();
            double Y = xbox.getY();
            double Z = xbox.getZ();
            double rot = xbox.getRawAxis(4);
           if (Math.abs(X) < 0.2) {
                X = 0;
            }
            if (Math.abs(Y) < 0.2) {
                Y = 0;
            }
            if (Math.abs(rot) < 0.2) {
                rot = 0;
            }
            if (Math.abs(Z) < 0.2) {
                Z = 0;
            }
//            System.out.println("X="+X+" and Y="+Y);
                mydrive.mecanumDrive_Cartesian(X, Y, rot, 0);
            } catch (Exception e) {
                System.out.println(e.getMessage());
            }
//System.out.println(("3x="+X+ "  Y="+Y+"   rot="+rot));

//            if (last != 0) {
//                double nax = accel.getAcceleration(ADXL345_I2C.Axes.kX);
//                double nay = accel.getAcceleration(ADXL345_I2C.Axes.kY);
//                double az = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
//
//                if (iocount % 100 == 0) {
//                    System.out.println("accelrometer values = (" + nax + ", "
//                            + nay + ", " + az + ")");
//                }
//
//                double ax = nax * Math.cos(ang) - nay * Math.sin(ang);
//                double ay = nax * Math.sin(ang) + nay * Math.cos(ang);
//
//                long newVal = System.currentTimeMillis();
//
//                double tDiff = (newVal - last) / 1000.0;
//
//                vx += ax * 3.2808399 * tDiff;
//                vy += ay * 3.2808399 * tDiff;
//                vz += az * 3.2808399 * tDiff;
//
//                x += vx * tDiff;
//                y += vy * tDiff;
//                z += vz * tDiff;
//
//                last = newVal;
//
//            } else {
//                last = System.currentTimeMillis();
//            }

//            if (iocount % 10 == 0) {
////                System.out.println("(" + x + ", " + y + ")");
//                try  {
//                    System.out.println(Math.abs(frontRight.getOutputCurrent())+Math.abs(backLeft.getOutputCurrent())+Math.abs(backRight.getOutputCurrent())+Math.abs(frontLeft.getOutputCurrent()));
//                } catch (CANTimeoutException e) {
//                    
//                }
//            }
//
//            Timer.delay(loopinterval);
//            time += loopinterval;
//            iocount++;
        }
       
System.out.println("Out of main loop");
//      stop motors
        try {
            frontRight.setX(0.0);
            backRight.setX(0.0);
            backLeft.setX(0.0);
            frontLeft.setX(0.0);
        } catch (Exception e) {
        }
        
        
    }
    
    public void Shoot(double shooterSpeed) {
        try {
            // currently this locks the robot
            frontRight.setX(0);
            backRight.setX(0);
            backLeft.setX(0);
            frontLeft.setX(0);

            int count = 0;
            try {
                // we should turn to correct location here
                //double shooterSpeed=myCourt.getPowerNeeded();
                shooter1.setX(-shooterSpeed);
                shooter2.setX(-shooterSpeed);
                shooter3.setX(-shooterSpeed);
                while (true) {
                    if (Math.abs((shooter1.getSpeed() - shooterSpeed) / shooterSpeed) < 0.015
                            && Math.abs((shooter2.getSpeed() - shooterSpeed) / shooterSpeed) < 0.015
                            && Math.abs((shooter3.getSpeed() - shooterSpeed) / shooterSpeed) < 0.015) {
                        thruster.setX(-15.0);
                        Timer.delay(1);
                        thruster.setX(0);
                        break;
                    }
                    if (count++ >= 100) {
                        Timer.delay(.5);
                        break;
                    }
                    Timer.delay(.05);
                }
                shooter1.setX(0);
                shooter2.setX(0);
                shooter3.setX(0);
            } catch (CANTimeoutException ex) {
                ex.printStackTrace();
            }
            
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
        
    }
}
